#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include "axisgroup/axisgroup.h"
#include "axisgroup/fsm_booting.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup_robot.h"

using namespace axisgroup;
class MockAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
};
class FsmBootingTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        for (int i = 0; i < 7; i++) { EXPECT_CALL(j[i], get_id()).WillRepeatedly(testing::Return(i)); }
    }
    virtual void TearDown() override {}

    axisgroup::AxisGroup ag;
    MockAxis j[7];
};

TEST_F(FsmBootingTest, create)
{
    ASSERT_EQ(ag.get_serial_num(), axisgroup::DEFAULT_SERIAL_NUM);
    ASSERT_EQ(ag.joint_num(), 0);
    ASSERT_EQ(ag.get_state(), axisgroup::AxisGroup::FsmState::Booting);
}

TEST_F(FsmBootingTest, linkjoint)
{
    ASSERT_EQ(ag.get_serial_num(), axisgroup::DEFAULT_SERIAL_NUM);
    ASSERT_EQ(ag.joint_num(), 0);

    ASSERT_EQ(ag.link_joint(&j[0]), 0);
    ASSERT_EQ(ag.get_joint_ids().size(), 1);
    ASSERT_EQ(ag.link_joint(&j[0]), -2);
    ASSERT_EQ(ag.get_joint_ids().size(), 1);

    ASSERT_EQ(ag.link_joint(&j[4]), 0);
    ASSERT_EQ(ag.get_joint_ids().size(), 2);

    ASSERT_EQ(ag.link_joint(&j[1]), 0);
    ASSERT_EQ(ag.link_joint(&j[5]), 0);

    auto jids = ag.get_joint_ids();
    EXPECT_EQ(jids.size(), 4);
    EXPECT_EQ(jids[0], 0);
    EXPECT_EQ(jids[1], 1);
    EXPECT_EQ(jids[2], 4);
    EXPECT_EQ(jids[3], 5);

    ASSERT_EQ(ag.link_joint(&j[2]), 0);
    ASSERT_EQ(ag.link_joint(&j[3]), 0);
    ASSERT_EQ(ag.link_joint(&j[6]), -1);

    jids = ag.get_joint_ids();
    EXPECT_EQ(jids.size(), 6);
    for (int i = 0; i < 6; i++) { EXPECT_EQ(jids[i], i); }
}

class MockBootingSta : public axisgroup::FsmBooting
{
public:
    MockBootingSta(axisgroup::AxisGroup* grp) : axisgroup::FsmBooting(grp)
    {
        EXPECT_CALL(*this, sync_boot_data()).WillRepeatedly(testing::Return(-1));
        EXPECT_CALL(*this, sync_dh_data()).WillRepeatedly(testing::Return(-1));
        EXPECT_CALL(*this, sync_dyna_data()).WillRepeatedly(testing::Return(-1));
    }
    MOCK_METHOD0(sync_boot_data, int());
    MOCK_METHOD0(sync_dh_data, int());
    MOCK_METHOD0(sync_dyna_data, int());
    MOCK_METHOD0(timeout_action, void());
};

TEST_F(FsmBootingTest, check_axis)
{
    auto booting = std::make_shared<MockBootingSta>(&ag);
    booting->update(true);
    ASSERT_EQ(booting->boot_stat_, 1);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 1);

    ag.link_joint(j + 0);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 1);

    ag.link_joint(j + 1);
    ag.link_joint(j + 5);
    EXPECT_CALL(j[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::ErrorStop));
    ag.link_joint(j + 3);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 1);

    EXPECT_CALL(j[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    ag.link_joint(j + 2);
    ag.link_joint(j + 4);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 2);
}

TEST_F(FsmBootingTest, sync_boot_data)
{
    auto booting = std::make_shared<MockBootingSta>(&ag);
    for (int i = 0; i < 6; i++) ag.link_joint(j + i);
    booting->update(true);
    ASSERT_EQ(booting->boot_stat_, 2);

    EXPECT_CALL(*booting, sync_boot_data()).WillOnce(testing::Return(-1));
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 2);

    EXPECT_CALL(*booting, sync_boot_data()).WillOnce(testing::Return(0));
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 3);
}

TEST_F(FsmBootingTest, sync_dh_data)
{
    auto booting = std::make_shared<MockBootingSta>(&ag);
    for (int i = 0; i < 6; i++) ag.link_joint(j + i);
    EXPECT_CALL(*booting, sync_boot_data()).WillOnce(testing::Return(0));
    booting->update(true);
    ASSERT_EQ(booting->boot_stat_, 2);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 3);

    EXPECT_CALL(*booting, sync_dh_data()).WillOnce(testing::Return(-1));
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 3);

    EXPECT_CALL(*booting, sync_dh_data()).WillOnce(testing::Return(0));
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 4);

    EXPECT_CALL(*booting, sync_dh_data()).Times(0);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 4);
}

TEST_F(FsmBootingTest, sync_dyna_data)
{
    auto booting = std::make_shared<MockBootingSta>(&ag);
    for (int i = 0; i < 6; i++) ag.link_joint(j + i);
    EXPECT_CALL(*booting, sync_boot_data()).WillOnce(testing::Return(0));
    EXPECT_CALL(*booting, sync_dh_data()).WillOnce(testing::Return(0));
    booting->update(true);
    ASSERT_EQ(booting->boot_stat_, 2);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 3);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 4);

    booting->update();
    ASSERT_EQ(booting->boot_stat_, 4);

    EXPECT_CALL(*booting, sync_dyna_data()).WillOnce(testing::Return(0));
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 5);
    ag.update_fsm();
    ASSERT_EQ(ag.get_state(), axisgroup::AxisGroup::FsmState::Disabled);

    EXPECT_CALL(*booting, sync_dyna_data()).Times(0);
    booting->update();
    ASSERT_EQ(booting->boot_stat_, 5);
}
TEST_F(FsmBootingTest, booting_timeout)
{
    auto booting = std::make_shared<MockBootingSta>(&ag);

    ASSERT_EQ(booting->watchdog_cnt_, 30000);
    booting->update(true);
    ASSERT_EQ(booting->watchdog_cnt_, 29992);

    EXPECT_CALL(*booting, timeout_action()).Times(0);
    booting->watchdog_cnt_ = 16;
    booting->update();

    ASSERT_EQ(booting->watchdog_cnt_, 8);
    ASSERT_EQ(booting->boot_stat_, 1);
    EXPECT_CALL(*booting, timeout_action()).Times(1);
    booting->update();
    ASSERT_EQ(booting->watchdog_cnt_, 0);
    ASSERT_EQ(booting->boot_stat_, 1);
    booting->update();
    ag.update_fsm();
    ASSERT_EQ(ag.get_state(), axisgroup::AxisGroup::FsmState::Booting);

    ASSERT_EQ(booting->watchdog_cnt_, 0);
    ASSERT_EQ(booting->boot_stat_, 1);
    ag.update_fsm();
    ASSERT_EQ(ag.get_state(), axisgroup::AxisGroup::FsmState::Booting);
}